/*******************************************************************
 *
 * Name        	: my_test.cpp
 * Author      	: tcl
 * Version     	:
 * Description 	: RTOS based empty executable project
 *
 ********************************************************************/

#include <stdio.h>
#include <stdlib.h>


extern "C" {
#include "qurt_thread.h"
void test_contours();
}


#include <hexagon_types.h>
#include <hexagon_protos.h>
#include <hvx_hexagon_protos.h>

#include <qhmath.h>
#include <qhcomplex.h>

#define FARF_HIGH 1
#include "HAP_farf.h"
#include "HAP_perf.h"

float contours_area_hvx(const uint32_t * __restrict pContourPoints, uint32_t numPoints)
{
    int32_t area = 0;
    int32_t prev_x = pContourPoints[2*(numPoints - 1)];
    int32_t prev_y = pContourPoints[2*(numPoints - 1) + 1];
    HVX_Vector *v_ptr = (HVX_Vector*)pContourPoints;
    HVX_Vector v_area = Q6_V_vzero();
    int32_t area_buf[32];
    int32_t remain = numPoints % 32;
    int32_t i = 0;
    for (i = 0; i < numPoints - remain; i += 32)
    {
        // HVX_Vector v = *v_ptr;
        HVX_Vector v1 = *v_ptr++; // w, x,y,x,y...
        HVX_Vector v2 = *v_ptr++;
        HVX_Vector v = Q6_Vh_vpacke_VwVw(v2, v1); // h, x,y,x,y...
        // HVX_Vector v1 = Q6_Vh_vdeal_Vh(v); // h, y,y,...,x,x...
        HVX_VectorPair v_pair = Q6_Wuw_vzxt_Vuh(v); // w, L: x,x,..., H: y,y,...
        HVX_Vector x = Q6_V_lo_W(v_pair);
        HVX_Vector y = Q6_V_hi_W(v_pair);
        // int rs1 = (0) << 2;
        // int rs2 = (31) << 2;
        // int x_r1 = Q6_R_vextract_VR(x, rs1);
        // int x_r2 = Q6_R_vextract_VR(x, rs2);
        // int y_r1 = Q6_R_vextract_VR(y, rs1);
        // int y_r2 = Q6_R_vextract_VR(y, rs2);
        // FARF(RUNTIME_HIGH, "v1: %ld, %ld, %ld, %ld", x_r1,  x_r2, y_r1, y_r2);
        v1 = Q6_V_vsplat_R(prev_x);
        v2 = Q6_V_vsplat_R(prev_y);
        HVX_Vector v_prev_x = Q6_V_vlalign_VVI(x, v1, 4);
        HVX_Vector v_prev_y = Q6_V_vlalign_VVI(y, v2, 4);
        // x_r1 = Q6_R_vextract_VR(v_prev_x, rs1);
        // x_r2 = Q6_R_vextract_VR(v_prev_x, rs2);
        // y_r1 = Q6_R_vextract_VR(v_prev_y, rs1);
        // y_r2 = Q6_R_vextract_VR(v_prev_y, rs2);
        // FARF(RUNTIME_HIGH, "v2: %ld, %ld, %ld, %ld", x_r1,  x_r2, y_r1, y_r2);

        v1 = Q6_Vw_vdmpy_VhVh_sat(v_prev_x, y);
        v2 = Q6_Vw_vdmpy_VhVh_sat(v_prev_y, x);
        // x_r1 = Q6_R_vextract_VR(v1, rs1);
        // x_r2 = Q6_R_vextract_VR(v1, rs2);
        // y_r1 = Q6_R_vextract_VR(v2, rs1);
        // y_r2 = Q6_R_vextract_VR(v2, rs2);
        // FARF(RUNTIME_HIGH, "v3: %ld, %ld, %ld, %ld", x_r1,  x_r2, y_r1, y_r2);
        v = Q6_Vw_vadd_VwVw(v_area, v1);
        v_area  = Q6_Vw_vsub_VwVw(v, v2);
        // x_r1 = Q6_R_vextract_VR(v_area, rs1);
        // x_r2 = Q6_R_vextract_VR(v_area, rs2);
        // FARF(RUNTIME_HIGH, "v4: %ld, %ld, %ld, %ld", x_r1,  x_r2, y_r1, y_r2);

        // prev_x = pContourPoints[2*(i+31)];
        // prev_y = pContourPoints[2*(i+31)+1];
        int rs = (31) << 2;
        prev_x = Q6_R_vextract_VR(x, rs);
        prev_y = Q6_R_vextract_VR(y, rs);
    }
#if 0
     HVX_VectorPred Qv = Q6_Q_vsetq2_R(128);
     Q6_vmem_QRIV(Qv, (HVX_Vector*)area_buf, v_area);
#else
    *(HVX_Vector*)area_buf = v_area;
#endif
    for (int j = 0; j < 32; j++)
    {
        area += area_buf[j];
    }
    for (; i < numPoints; i++) {
        int32_t x = pContourPoints[2*i];
        int32_t y = pContourPoints[2*i+1];
        area += prev_x * y - prev_y * x;
        prev_x = x;
        prev_y = y;
    }
//    return area/2.f;
    return qhmath_abs_f(qhmath_div_f(area, 2.f));
}

uint32_t __attribute__ ((__aligned__(128))) contour_points[] = {
  289,163,
  290,162,
  291,162,
  292,162,
  293,163,
  294,163,
  295,163,
  296,164,
  297,165,
  297,166,
  298,167,
  298,168,
  298,169,
  298,170,
  297,171,
  297,172,

  296,173,
  295,174,
  294,175,
  293,175,
  292,175,
  291,176,
  290,175,
  289,175,
  288,175,
  287,174,
  286,173,
  285,172,
  285,171,
  285,170,
  285,169,
  285,168,

  285,167,
  286,166,
  286,165,
  287,164,
  288,163,
};

void test_contours()
{
	float area;
	uint32_t num = sizeof(contour_points)/sizeof(uint32_t)/2;
	uint64_t ts1 = HAP_perf_get_time_us();
	area = contours_area_hvx(contour_points, num);
	uint64_t ts2 = HAP_perf_get_time_us();
	FARF(RUNTIME_HIGH, "=============== HVX DSP: area %f ====%llu us===========", area, ts2-ts1);
}
